import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch.conditions import IfCondition
from launch_ros.actions import Node

def generate_launch_description():
    my_pkg_dir = get_package_share_directory('b2_nav_bringup')
    collision_monitor_dir = get_package_share_directory('nav2_collision_monitor')
    
    map_file = os.path.join(my_pkg_dir, 'map', 'DAM-563_11.yaml')
    params_file = os.path.join(my_pkg_dir, 'params', 'nav2_params.yaml')

    use_sim_time = LaunchConfiguration('use_sim_time')
    declare_use_sim_time = DeclareLaunchArgument('use_sim_time', default_value='false')
    
    map_yaml = LaunchConfiguration('map')
    declare_map_yaml = DeclareLaunchArgument('map', default_value=map_file)

    rviz_config_file = os.path.join(my_pkg_dir, 'rviz', 'nav2_default_view.rviz')
    declare_rviz_config_file = DeclareLaunchArgument('rviz_config_file', default_value=rviz_config_file)
    
    nav_params = LaunchConfiguration('params_file')
    declare_params_file = DeclareLaunchArgument('params_file', default_value=params_file)
    
    autostart = LaunchConfiguration('autostart')
    declare_autostart = DeclareLaunchArgument('autostart', default_value='true')

    use_rviz = LaunchConfiguration('use_rviz')
    declare_use_rviz = DeclareLaunchArgument(
        'use_rviz', 
        default_value='true',
        description='Whether to start Rviz2')

    launch_odom_tf = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(my_pkg_dir, 'launch', 'odom_tf_pub.launch.py')),
        launch_arguments={'use_sim_time': use_sim_time}.items()
    )

    launch_pcl2scan = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(my_pkg_dir, 'launch', 'sample_pointcloud_to_laserscan_launch.py')),
        launch_arguments={'use_sim_time': use_sim_time}.items()
    )

    collision_monitor_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(collision_monitor_dir, 'launch', 'collision_monitor_node.launch.py')
        ),
        launch_arguments={
            'use_sim_time': use_sim_time,
            'params_file': params_file
        }.items()
    )

    launch_localization = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(my_pkg_dir, 'launch', 'localization_launch.py')),
        launch_arguments={
            'map': map_yaml,
            'use_sim_time': use_sim_time,
            'autostart': autostart,
            'params_file': nav_params,
            'use_composition': 'False'
        }.items()
    )

    launch_navigation = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(my_pkg_dir, 'launch', 'navigation_launch.py')),
        launch_arguments={
            'use_sim_time': use_sim_time,
            'autostart': autostart,
            'params_file': nav_params,
            'use_composition': 'False'
        }.items()
    )

    rviz_node = Node(
        condition=IfCondition(use_rviz),
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        arguments=['-d', rviz_config_file],
        output='screen'
    )

    ld = LaunchDescription()
    ld.add_action(declare_use_sim_time)
    ld.add_action(declare_map_yaml)
    ld.add_action(declare_params_file)
    ld.add_action(declare_autostart)
    ld.add_action(declare_use_rviz)
    ld.add_action(declare_rviz_config_file)
    
    ld.add_action(launch_odom_tf)
    ld.add_action(launch_pcl2scan)
    ld.add_action(collision_monitor_launch)
    ld.add_action(launch_localization)
    ld.add_action(launch_navigation)
    ld.add_action(rviz_node)

    return ld